#include "fifo.h"

static inline uint8_t fifo_next_index(uint8_t index) {
    return (index + 1) % FIFO_BUFFER_SIZE;
}

void fifo_init(fifo_buffer_t *fifo) {
    fifo->head = 0;
    fifo->tail = 0;
    fifo->full = false;
}

bool fifo_is_empty(fifo_buffer_t *fifo) {
    return (!fifo->full && fifo->head == fifo->tail);
}

bool fifo_is_full(fifo_buffer_t *fifo) {
    return fifo->full;
}

uint8_t fifo_available(fifo_buffer_t *fifo) {
    if (fifo->full) {
        return FIFO_BUFFER_SIZE;
    }
    if (fifo->head >= fifo->tail) {
        return fifo->head - fifo->tail;
    } else {
        return FIFO_BUFFER_SIZE + fifo->head - fifo->tail;
    }
}

bool fifo_check_space(fifo_buffer_t *fifo, uint8_t required_len) {
    uint8_t available = FIFO_BUFFER_SIZE - fifo_available(fifo);
    return (available >= required_len);
}

uint8_t fifo_peek(fifo_buffer_t *fifo, uint8_t *buf, uint8_t len) {
    uint8_t available = fifo_available(fifo);
    if (len > available) {
        len = available;
    }

    for (uint8_t i = 0; i < len; i++) {
        buf[i] = fifo->buffer[(fifo->tail + i) % FIFO_BUFFER_SIZE];
    }

    return len;
}

void fifo_consume(fifo_buffer_t *fifo, uint8_t len) {
    uint8_t available = fifo_available(fifo);
    if (len > available) {
        len = available;
    }

    fifo->tail = (fifo->tail + len) % FIFO_BUFFER_SIZE;
    if (fifo->tail == fifo->head) {
        fifo->full = false;
    }
}

bool fifo_write(fifo_buffer_t *fifo, uint8_t data) {
    if (fifo->full) {
        return false; // 已满，无法写入
    }

    fifo->buffer[fifo->head] = data;
    fifo->head = fifo_next_index(fifo->head);

    if (fifo->head == fifo->tail) {
        fifo->full = true;
    }

    return true;
}

bool fifo_read(fifo_buffer_t *fifo, uint8_t *data) {
    if (!fifo->full && fifo->head == fifo->tail) {
        return false; // 空，无法读取
    }

    *data = fifo->buffer[fifo->tail];
    fifo->tail = fifo_next_index(fifo->tail);
    fifo->full = false;

    return true;
}

uint8_t fifo_write_buf(fifo_buffer_t *fifo, const uint8_t *data, uint8_t len) {
    // 先检查是否有足够空间容纳整个 len 字节
    if (!fifo_check_space(fifo, len)) {
        return 0; // 空间不足，不写入任何数据
    }

    // 空间足够，开始写入
    for (uint8_t i = 0; i < len; i++) {
        // 因为我们已经检查过空间，理论上不会失败
        fifo_write(fifo, data[i]);
    }

    return len; // 成功写入全部字节
}

uint8_t fifo_read_buf(fifo_buffer_t *fifo, uint8_t *buf, uint8_t bufsize) {
    uint8_t read = 0;
    while (read < bufsize && !fifo_is_empty(fifo)) {
        if (!fifo_read(fifo, &buf[read])) {
            break;
        }
        read++;
    }
    return read;
}